/*
 *  masslink.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 06/08/2012
 *
 */

#include "masslink.h"

MassLink::MassLink()
:	Component(),
	initialDis(0.0),
	areaIni(1.0),
	mass(0.0),
	material()
{}

// ***

MassLink::MassLink(vector<string>& vp, double a, string& mn, int di)
:	Component(vp,mn,di),
	areaIni(a),
	material()
{
	mass = -1.0;	// Mass will be computed later
}

// ***

MassLink::MassLink(vector<string>& vp, double masa, int di)
:	Component(vp,"",di),
	areaIni(),
	material(),
	mass(masa)
{}

// ***

void MassLink::calcFi(const vector<double>& qddot)
{
	int i;
	int j;
	
	for(i=0; i<numDofs; ++i)
	{
		Fi[i] = 0.0;
		for(j=0; j<numDofs; ++j)	
		{
			if(integratorType==0)  Fi[i] += M(i,j) *qddot[ localGlob[j] ];
			else	Fi[i] += M(i,j) * (dq[j] - dqC[j]);
		}
	}
//	cout << "q(0) = " << q[0] << endl;
//	cout << "q(1) = " << q[1] << endl;
//	cout << "Fi es = " << endl;
//	printSTLVector(Fi);
}

// ***

void MassLink::calcFq(const int& nt)
{}

// ***

void MassLink::calcFt(const double& t, const double& grav, const int& gravDir)
{}

// ***

void MassLink::calcKi(double & partial_qddot)
{
	if(partial_qddot!=partial_qddotOld)	// In order to compute Ki just once
	{
//		int i;
//		int j;
//		for(i=0; i<numDofs; i++)
//			for(j=0; j<numDofs; j++) Ki[i][j] = M(i,j)*partial_qddot;
		Ki = M * partial_qddot;

	}	
	partial_qddotOld = partial_qddot;
}

// ***

void MassLink::calcKq(int nt)
{}	

// ***

void MassLink::calcKdq(double & partial_qdot)
{}

// ***

void MassLink::calcM(const vector<double>& q, const double& t)
{
	int i;
	if(M.size()==0)
	{	
		M.setZero(numDofs,numDofs);
	}
	// Consistent mass matrix
	if(numDofs==6)	// dim=3
	{
		M(0,0) = mass/3.;
		M(0,3) = mass/6.; 
		M(1,1) = mass/3.; 
		M(1,4) = mass/6.;
		M(2,2) = mass/3.;
		M(2,5) = mass/6.;
		M(3,0) = M(0,3);
		M(3,3) = mass/3.;
		M(4,1) = M(1,4); 
		M(4,4) = mass/3.;
		M(5,2) = M(2,5);
		M(5,5) = mass/3.;	
	} 
	else if(numDofs==4)	// dim=2
	{
		M(0,0) = mass/3.;
		M(0,2) = mass/6.;
		M(1,1) = mass/3.;
		M(1,3) = mass/6.;
		M(2,0) = M(0,2);
		M(2,2) = mass/3.;
		M(3,1) = M(1,3);
		M(3,3) = mass/3.;
	} 
	else			// dim=1
	{
		M(0,0) = mass/3.;
		M(0,1) = mass/6.;
		M(1,0) = M(0,1);
		M(1,1) = mass/3.;
	}
// 	printEigenMatrixXd(M);
}

// ***

void MassLink::calcMLumped(const vector<double>& q, const double& t)
{
	if (MLumped.size()==0)
	{
		MLumped.resize(numDofs);
	}
	
	int i;
	for (i=0; i<numDofs; ++i)	MLumped[i] = mass/2.0;
}

// ***

void MassLink::setDofs(void)
{
	map<string,Point*,less<string> >::iterator i;
	for(i=mPoints.begin(); i!=mPoints.end(); i++)	/** Loop on points belonging to this component */
	{
		for(int j=0; j<dim; j++)	(*i).second->setDof(0,j);	// Only translational dof */
		for(int j=0; j<dim; j++)	(*i).second->setDofInertia(0,j);
	}
	numDofs=dim*pointNames.size();
	numDofsInertia = numDofs;
}

// ***

void MassLink::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{
	int i;
	int j;
	int p;
	
	// Initial position
	q.resize(numDofs);
	
	i=0;
	for(p=0; p<pointNames.size(); ++p)
	{
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[i*dim+j] ] = mPoints[pointNames[p]]->r[j];
		}
		i++;
	}
	
	// WARNING:
	// Remember that the ordering of vector pointNames and map mPoints is different
	initialDis = lTwoNorm(mPoints[pointNames[0]]->r, mPoints[pointNames[1]]->r);
	
	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
	
	if (mass==-1)
	{
		mass = initialDis*areaIni*(material->getDensity());
	}
}

// ***

void MassLink::setMatAtComp(Material* ma)
{
	material = ma;
}

// ***

void MassLink::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{	
	if(grav!=0.0)
	{
		int i;
		int j;		
		Ft.resize(numDofs);
		// Build Ft once for all  Ft= mass * g
		Ft[gravDir-1] = - mass * grav / 2.0;
		Ft[dim+gravDir-1] = - mass * grav / 2.0;
	}
	
	if(st!=0)
	{
	  Fi.resize(numDofs);
	  M.setZero(numDofs,numDofs);
	}
}

// ***

void MassLink::initializeCompTang(const bool& st)
{
	int i;
	
	if(st!=0)
	{		
//		Ki.resize(numDofs);
//		for(i=0; i<numDofs;++i)	Ki[i].resize(numDofs);
		Ki.setZero(numDofs,numDofs);
	}	
}

// ***

void MassLink::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	int i;
	// Kinetic
	if(dq.size()!=0 && (M.size()!=0 || MLumped.size()!=0) )
	{
		int j;
		for (i=0; i<numDofs; ++i)	dq[i] = dq_in[localGlob[i]];
		for (i=0; i<numDofs; ++i)
		{
			if (M.size()!=0)	// Consistent
			{
				for (j=0; j<numDofs; ++j)
				{
					Ekin+= 0.5 * dq[i] * M(i,j) * dq[j];
				}
			}
			else	// Lumped
			{
				Ekin+= 0.5 * dq[i] * MLumped[i] * dq[i];
			}						
		}
	}
	// Potential
	
	for (i=0; i<numDofs; ++i)
	{
		q[i] = q_in[localGlob[i]];
	}
	
	if (grav != 0.0) Epot += mass * grav * (q[gravDir - 1] + q[gravDir - 1 + dim]) / 2.0;  
}

// ***

void MassLink::calcEntropy(double& Entropy, double& EntropyAmb)
{}

// ***

void MassLink::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}	

// ***

void MassLink::updateComp(void)
{}

// ***

void MassLink::setIniDofs(vector<double>&)
{}

// ***

void MassLink::calcPhi(const vector<double>&)
{}

// ***

void MassLink::calcDPhi(const vector<double>&)
{}

// ***

void MassLink::calcMSmooth(const vector<double>& q_in, const double& t)
{
	int i;
	int j;
	
	MSmooth.setZero(numDofsInertia, numDofsInertia);

//	if(MSmooth.size()==0)	// Just in case M has not been created in initializeCompRes
//	{
//		MSmooth.resize(numDofsInertia);
//		for(i=0; i<numDofsInertia;++i)	MSmooth[i].resize(numDofsInertia);
//	}
	
	
	if (M.size()==0)  calcM(q,0.0);	// Just in case mass matrix not available

	if (material->getDensity()!=0.0)
	{
		for(i=0; i < numDofs; i++) 
		{
			for(j=0; j < numDofs; j++) 
			{
				MSmooth(i,j) = M(i,j)/(material->getDensity());
			}
		}
	}
	else
	{
	  	for(i=0; i < numDofs; i++) 
		{
			for(j=0; j < numDofs; j++) 
			{
				MSmooth(i,j) = M(i,j);
			}
		}
	}
	/*
	cout << "MSmooth is " << endl;
	for (i=0; i<numDofsInertia; ++i)
	{
		for (j=0; j<numDofsInertia; ++j)
		{
			cout << MSmooth[i,j) << " ";
		}
		cout << endl;
	}
	*/
	/*
	cout << "Diagonal de MSmooth is " << endl;
	for (i=0; i<numDofsInertia; ++i)
	{
		cout << MSmooth[i,i) << " ";
	}
			cout << endl;
	*/
}

// ***

void MassLink::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{
	FSmooth.setZero(2,numDofsInertia);
//	int i;
//	if (FSmooth.size()==0)		// Just in case M has not been created in initializeCompRes
//	{
//		FSmooth.resize(2);
//		for (i=0; i<2; ++i)	FSmooth[i].resize(numDofsInertia);
//	}
//	else setZero(FSmooth);
}

int MassLink::checkState()
{
	return 0;
}
